use super::{ConnReceiver, ConnSender};
use crate::common::thread;
use async_ach_spsc as spsc;
use core::slice;
use nrf_softdevice::ble::*;
use nrf_softdevice::{raw, Softdevice};

const SCAN_CFG: central::ScanConfig = central::ScanConfig {
    whitelist: None,
    extended: true,
    active: false,
    phys: PhySet::M1,
    interval: 1760, //1.1s
    window: 1760,   //1.1s
    timeout: 110,   //1.1s
    tx_power: TxPower::Minus4dBm,
};

pub fn start_scan() -> ConnReceiver {
    let (tx, rx) = spsc::channel();
    thread::spawn(scan(tx));
    rx
}

#[embassy::task]
async fn scan(mut tx: ConnSender) -> ! {
    let sd = unsafe { Softdevice::steal() };

    loop {
        let res = central::scan(sd, &SCAN_CFG, |params| unsafe {
            let mut data = slice::from_raw_parts(params.data.p_data, params.data.len as usize);
            while data.len() != 0 {
                let len = data[0] as usize;
                if len < 1 || data.len() < len + 1 {
                    break;
                }
                let key = data[1];
                let value = &data[2..len + 1];

                if key == 0xFF && value[0] == 0x64 && value[1] == 0x64 {
                    let address = Address::from_raw(params.peer_addr);
                    return Some(address);
                }
                data = &data[len + 1..];
            }
            None
        })
        .await;

        let address = if let Ok(res) = res {
            res
        } else {
            continue;
        };
        log::info!("Scan found address {:?}", address);
        let addrs = &[&address];

        let mut scan_config = SCAN_CFG;
        scan_config.whitelist = Some(addrs);
        let config = central::ConnectConfig {
            att_mtu: None,
            scan_config: scan_config,
            conn_params: raw::ble_gap_conn_params_t {
                min_conn_interval: 40, //50ms
                max_conn_interval: 48, //60ms
                slave_latency: 0,
                conn_sup_timeout: 60, // 600ms
            },
        };
        let result = central::connect(sd, &config).await;
        if let Ok(conn) = result {
            tx.send(conn).await;
        }
    }
}
